# 这里尝试利用gazebo实现基于diablo模型的仿真

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import Node


def generate_launch_description():

    shared_pkg = get_package_share_directory('imu_tool')

    #######################
    # Declare Arguments
    ######################
    imu_topic = LaunchConfiguration('imu_topic')
    declare_imu_topic = DeclareLaunchArgument(
        name='imu_topic',
        # default_value="/imu/sensor_msgs/Imu",
        default_value="/imu/data",
        description='Absolute path to imu topic')

    have_orientation = LaunchConfiguration('have_orientation')
    declare_have_orientation = DeclareLaunchArgument(
        name='have_orientation',
        # default_value="false",
        default_value="true",
        description='Whether the imu have orientation message')


    rviz_config_file = LaunchConfiguration('rviz_config')
    declare_rviz_config = DeclareLaunchArgument(
        name = 'rviz_config',
        default_value="/home/daybeha/Documents/diablo_ws/two-wheel-leg-robot/location/imu_tool/rviz/imu.rviz",
        description='Absolute path to rviz config file to load')

    ######################
    # Specify the actions
    ######################
    complementary_filter_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(shared_pkg, 'launch', 'complementary_filter_launch.py')),
    )

    static_tf_cmd = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_transform_publisher',
        output='screen',
        arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']
    )

    imu2odom_cmd = Node(
        package='imu_tool',
        executable='imu2odomtf',
        name='imu2odom_node',
        output='screen',
        parameters=[{'imu_topic': imu_topic}, {'have_orientation', have_orientation}],
    )

    rviz_cmd = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', rviz_config_file]
    )



    return LaunchDescription([
        declare_imu_topic,
        declare_have_orientation,
        declare_rviz_config,

        complementary_filter_cmd,
        static_tf_cmd,
        imu2odom_cmd,
        rviz_cmd,
    ])
